#include "SimpleSerial.cpp"
#include <string>
#include <iostream>
#include <time.h>
using namespace std;

typedef char byte;


/***********************************************************
 *Arduino Commands:
 *
 *These are the commands which the arduino will execute.
 *The pattern to construct a command is:
 *      AACCCCDDDD;
 *Where AA is the instruction identifier, CCCC is the first
 *argument in the instruction, and DDDD is the second
 *argument in the instruction.
 *
 *All commands must terminate with a semicolon.
 *
 *An example of this:
 *To get pin 3 to become an analog output which pwm's at
 *128, we would send the following commands:
 *pm0003OUTP;
 *aw00030128;
 *
 *Motors have a speed range from 0-2000, where 1000 is
 *neutral. 
 *The left motor is pin 2 and the right motor is pin 3
 **********************************************************/

/*
// The left motor is pin 2 and the right motor is pin 3
char right_motor_forward[12] = "pt00031300;";  //length 11
char right_motor_reverse[12] = "pt00030700;";  //length 11
char right_motor_stop[12] = "pt00031000;";     //length 11
char left_motor_forward[12] = "pt00021300;";   //length 11
char left_motor_reverse[12] = "pt00020700;";   //length 11
char left_motor_stop[12] = "pt00021000;";      //length 11
char delay[8] = "dy1000;"; //length 7
*/

//The pattern for variable naming will be #XY where X is the
//robot's side, Y is the direction (f for forward, b for back),
//and # is the byte sequence to write in (i.e., rf1 should be
//written before rf2

//foward
byte lf0 = 0b00010100;
byte lf1 = 0b01010100;
byte rf0 = 0b10010100;
byte rf1 = 0b11010100;

//stop
byte ls0 = 0b00001111;
byte ls1 = 0b01101000;
byte rs0 = 0b10001111;
byte rs1 = 0b11101000;

//backward
byte lb0 = 0b00001010;
byte lb1 = 0b01111100;
byte rb0 = 0b10001010;
byte rb1 = 0b11111100;

//low
byte ll0 = 0b00000000;
byte ll1 = 0b01000000;
byte rl0 = 0b10000000;
byte rl1 = 0b11000000;

void TurnLeft(SimpleSerial* serial) {
  // left wheel back, right wheel forward
  serial->serial_write(&lb0, 1);
  serial->serial_write(&lb1, 1);
  serial->serial_write(&rf0, 1);
  serial->serial_write(&rf1, 1);

}

void TurnRight(SimpleSerial* serial) {
  // left wheel forward, right wheel back
  serial->serial_write(&lf0, 1);
  serial->serial_write(&lf1, 1);
  serial->serial_write(&rb0, 1);
  serial->serial_write(&rb1, 1);
}

void Reverse(SimpleSerial* serial) {
  // left wheel back, right wheel back
  serial->serial_write(&lb0, 1);
  serial->serial_write(&lb1, 1);
  serial->serial_write(&rb0, 1);
  serial->serial_write(&rb1, 1);
}

void Forward(SimpleSerial* serial) {
  // left wheel forward, right wheel back
  serial->serial_write(&lf0, 1);
  serial->serial_write(&lf1, 1);
  serial->serial_write(&rf0, 1);
  serial->serial_write(&rf1, 1);
}

void Stop(SimpleSerial* serial) {
  // left wheel forward, right wheel back
  serial->serial_write(&ls0, 1);
  serial->serial_write(&ls1, 1);
  serial->serial_write(&rs0, 1);
  serial->serial_write(&rs1, 1);
}

void FullSpeedBack(SimpleSerial* serial) {
 // left wheel back, right wheel back
  serial->serial_write(&ll0, 1);
  serial->serial_write(&ll1, 1);
  serial->serial_write(&rl0, 1);
  serial->serial_write(&rl1, 1);
}

void Wait(int seconds) {
  clock_t endwait;
  endwait = clock() + seconds * CLOCKS_PER_SEC;
  while (clock() < endwait) {}
}

int main(int argc, char **argv) {
  string input = "";
  SimpleSerial serial(argv[1], atoi(argv[2]));
  
  while (input != "quit") {
    cin >> input;
    
    if (input == "a")
       TurnLeft(&serial);
    if (input == "d")
       TurnRight(&serial);
    if (input == "w")
       Forward(&serial);
    if (input == "s")
       Reverse(&serial);
    if (input == "e")
       Stop(&serial);
    if (input == "f")
      FullSpeedBack(&serial);
    if (input == "q") {
       Stop(&serial);
       exit(0);
    }
    Wait(1);
  } 
     
}

